#ifndef KINEMATICS_MULTI_PIVOTS_H_
#define KINEMATICS_MULTI_PIVOTS_H_

// FIXME: remove ros
#include "rclcpp/rclcpp.hpp"
#include <chassis_controller/pivots.h>
#include "chassis_controller/kinematics.h"
#include <chassis_controller/speed_limiter.h>
namespace chassis_controller
{
    class MultiPivotsKinematics : public Kinematics
    {
    public:
        explicit MultiPivotsKinematics(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);
        void update(std::vector<Spivot_value> &pivot_value,
                    std::vector<Spivot_param> &pivot_param,
                    std::vector<Spivot_setpoint> &pivot_setpoint, const rclcpp::Duration &period,
                    bool ready_move);

        void getCommand(double vx, double vy, double va);

        void getVelocity(double vx, double vy, double va);

        /// @brief 传入转舵和限速参数
        /// @param pivot_num
        /// @param steer_pid_param
        /// @param limiter_liner
        /// @param limiter_steer
        void init(int pivot_num, pid_param steer_pid_param, SpeedLimiter limiter_liner, SpeedLimiter limiter_ang , SpeedLimiter limiter_steer);

        void updateParam(int pivot_num, pid_param steer_pid_param);

        void setCommand(std::vector<Spivot_value> &pivot_value,
                        std::vector<Spivot_param> &pivot_param,
                        std::vector<Spivot_setpoint> &pivot_setpoint,
                        const rclcpp::Duration &period);

        bool getSteeringFlag();

        bool getSteerErrFlag();

        bool getBrakerFlag();

        uint64_t getSteerCmdId();

        void setSteerCmdId(uint64_t steer_cmd_id);

        //   void setMountPose(double x, double y);
        //   void setRadius(double radius);
        //   void setTurningVelocitylimit(double velocity);
        //   void setSteerAngleState(double angle);
        //   void setAngularVelocityState(double vel);
        //   double getSteer();
        //   double getAngularVelocity();

    private:
        std::vector<bool> invert_flag_;
        std::vector<bool> target_invert_flag_;
        std::vector<double> liner_setpoint_;
        std::vector<double> angle_setpoint_;
        std::vector<double> angle_setpoint_last_valid;
        std::vector<PID> PID_vector_;

        std::vector<SpeedLimiter> limiter_steer_;
        std::vector<double> last0_cmd_steer_;
        std::vector<double> last1_cmd_steer_;

        std::vector<SpeedLimiter> limiter_liner_;
        std::vector<double> last0_cmd_liner_;
        std::vector<double> last1_cmd_liner_;
        std::vector<double>  reduce_factor_liner_;

        Sxya setpoint_;
        Sxya current_vel_;

        bool movement_;
        bool drive_request_,drive_request_old_,drive_req_trigger_;
        double no_drive_time_;
        bool standstill_;
        bool steering_ = false;
        bool steering_last_ = false;
        double steer_fall_time_;                
        bool steer_follow_err_;
        bool braker_open_;
        double steer_delay_ = 0.0;
        double brake_time_ = 0.5;
        bool enable_brake_control_ = false;

        int pivots_num_;

        double steer_tolerance_;
        double steer_follow_tolerance_;
        double start_time_;

        double max_liner_;
        double min_liner_;

        uint64_t steer_cmd_id_;
        uint64_t steer_cmd_id_valid_;
        double MINIMUM_TRANSLATIONAL_VELOCITY;
        bool USE_ANGLE_TOLERANCE;
        bool USE_LIMITED_STEER_ANGLE;
        // steer wheel relative origin position
        double pose_x_, pose_y_;
        // Wheel radius
        double radius_;
        double max_steer_;
        // motor rotational velocity and steer angle
        double motor_rotational_velocity_, servo_steer_angle_;
        int motor_direction_;
        // wheel velocity resolution
        double desired_wheel_mount_velocity_x_;
        double desired_wheel_mount_velocity_y_;

        void checkSteerDirection(double target, double actual, double max, double min, bool &positive_allowed, bool &negtive_allowed);

        double normAngle2(double angle);
        };
}
#endif
